#include "AntiGyro.h"

namespace Horizon{

// 存入获得的装甲板
void AntiGyro::getArmor(Armor &armor){
    armors.emplace_back(armor);
}

// 存入参数
void AntiGyro::getParam(GimbalPose &gm,CameraMode &cameramode,float v0){
    this->cameramode = cameramode;
    cur_gimbalpose = gm;
    this->v0 = v0;
}

// 检验数据是否有相关性
bool AntiGyro::judgeIsRelevant(){
    vector<Armor> armors_copy(armors);
    //armors_copy.clear();
    
    smoothDataFilter(armors_copy);
    
    // 回归方程的建立
    float x_ave = 0.5*(1 + armors_copy.size());

    vector<double> yaw_angle;
    for(size_t k = 0; k < armors_copy.size();k++){
        yaw_angle[k] = armors_copy[k].h_yaw_angle_;
    }
    float y_ave = average(yaw_angle);// 易出问题的地方

    float E = 0,F = 0,G = 0,H = 0;
    float I = 0,J = 0,L = 0;
    for(size_t k = 0;k < armors_copy.size();k++){
        E = (k - x_ave);
        I = I + E;

        L = L + (k - x_ave)*(k - x_ave);

        F = (armors_copy[k].h_yaw_angle_ - y_ave);
        J = J + F;

        G = G + E*F;
    }
    float b = G/L;
    float a = y_ave - b*x_ave;

    float R = 0;
    
    float M = 0,N = 0;
    for(size_t k = 0;k < armors_copy.size();k++){
        M = (yaw_angle[k] - y_ave)*(yaw_angle[k] - y_ave);  // 分子
        N = (yaw_angle[k] - b*k+a)*(yaw_angle[k] - b*k+a);  // 分母
    }
    R = 1 - N/M;
    if(abs(R) < 0.75){
        return false;
    }
    return true;
}

double AntiGyro::getAngle(Armor &armor){
    Vector3f top_center;//装甲板的上中心点
    Vector3f bottom_center;//装甲板的下中心点

    float x1 = (armor.left_up_.x + armor.right_up_.x)/2;
    float y1 = (armor.left_up_.y + armor.right_up_.y)/2;
    float z1 = (armor.left_up_.z + armor.right_up_.z)/2;

    top_center << x1,y1,z1;

    float x2 = (armor.left_down_.x + armor.right_down_.x)/2;
    float y2 = (armor.left_down_.y + armor.right_down_.y)/2;
    float z2 = (armor.left_down_.z + armor.right_down_.z)/2;

    bottom_center << x2,y2,z2;

    top_center = cam2ptz(cur_gimbalpose,top_center);
    bottom_center = cam2ptz(cur_gimbalpose,bottom_center);

    double angle = atan((top_center[0]-bottom_center[0])/(top_center[2]-bottom_center[2]));
    
    return angle;
}

Speed AntiGyro::getPalstance(){

    vector<Armor> armors_copy(armors);
    smoothDataFilter(armors_copy);

    double palstance;
    // 拟合直线公式 y = k * x + b ，其中 y 为 angle，x 为 次数，k 为 palstance
    double avg_x  = 0;
    double avg_x2 = 0;
    double avg_y  = 0;
    double avg_xy = 0;

    double first = 0;
    for (int k = 0; k < armors_copy.size(); k++)
    {
        avg_x  += k - first;
        avg_x2 += pow(k - first, 2);
        avg_y  += armors_copy[i].h_yaw_angle_;
        avg_xy += (k - first) * armors_copy[i].h_yaw_angle_;
    }
    avg_x  /= armors_copy.size();
    avg_x2 /= armors_copy.size();
    avg_y  /= armors_copy.size();
    avg_xy /= armors_copy.size();
    palstance = (avg_xy - avg_x * avg_y) / (avg_x2 - pow(avg_x, 2));

    std::cout << "[Top] calc_palstance: " << avg_x << " " << avg_x2 << " " << avg_y << " " << avg_xy << "->" << palstance << std::endl;
    Speed speed;
    speed.rate = palstance;
    if(palstance > MIN_RATE){
        speed.direction = -1;
    }else if(palstance < -MIN_RATE){
        speed.direction = 1;
    }else{
        speed.direction = 0;
    }
    
    return speed;
}

void AntiGyro::smoothDataFilter(vector<Armor> &list){
    for(size_t k = 0;k < list.size();k++){
        for(size_t m = 0;m < list.size()-1-k;m++){
            if(list[m].h_yaw_angle_ > list[m+1].h_yaw_angle_){
                swap(&list[m].h_yaw_angle_,&list[m+1].h_yaw_angle_);
            }
        }
    }
}
Armor AntiGyro::smoothDataFilter1(vector<Armor> &list){
    int l = 0;
    for(size_t k = 0;k < list.size();k++){
        for(size_t m = 0;m < list.size()-1-k;m++){
            if(list[m].area > list[m+1].area){
                l = m;
            }else{
                l = m+1;
            }
        }
    }
    return list[l];
}
float AntiGyro::average(vector<double> &list){
    float x_ave;
    for(size_t k = 0;k < list.size();k++){
        x_ave += list[k];
    }
    x_ave /= list.size();
    
    return x_ave;
}

float AntiGyro::bullteFlyTime(Armor &armor_fix){
    
    
    Vector3f pos;
    pos << armor_fix.center_.x,armor_fix.center_.y,armor_fix.center_.z;
    float g = 9.80665;
    float v0 = 10.0;
    float x1,y1,z1;

    //不是这的问题
    pos = cam2ptz(cur_gimbalpose,pos);
    //先算yaw的值的
    float distance1;
    distance1 = sqrt(pos[0]*pos[0]+pos[2]*pos[2]);
    return_gimbalpose.yaw = asin(pos[0]/distance1)*180/CV_PI;

    //pitch值,这里的问题。操，这塔玛都能报错,多赋值吧。。。。。。。。。。玛比。。。。。。
    float a = -0.5*g*(pow(distance1,2)/pow(v0,2));
    float b = distance1;
    float c = -((0.5*g*pow(distance1,2))/pow(v0,2) + pos[1]);

    float Discriminant = pow(b,2) - 4*a*c;  //判别式
    if(Discriminant < 0) return ;
    float tan_angle1 = (-b + pow(Discriminant,0.5)/(2*a));//*180/CV_PI;
    float tan_angle2 = (-b - pow(Discriminant,0.5)/(2*a));

    float angle1 = atan(tan_angle1)*180/CV_PI;
    float angle2 = atan(tan_angle2)*180/CV_PI;

    if(fabs(angle1) <= fabs(angle2) && fabs(angle1) < 45){
        return_gimbalpose.pitch = angle1;
        cout << angle1;
    }else if(fabs(angle2) < 45){
        return_gimbalpose.pitch = angle2;
        cout << angle2;
    }else{
        //cout << "计算解不符合实际" << endl;
        return -1;
    }

    return distance1/(v0*sin(return_gimbalpose.pitch));

}

}